//
// Created by 刘昱杉2020 on 2023/4/14.
//
#include "motor.h"
#include "tim.h"
#include "usart.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "stdbool.h"


/*****************内部函数声明***********************/
//统计马达的角度和速度
void Motor_motorTt_Cal(Motor_TT *motor);
//获取马达速度角度
void Motor_motorTt_GetSpeed(Motor_TT *motor);

/**********************内部工具函数*******************/

//统计马达的角度和速度
void Motor_motorTt_Cal(Motor_TT *motor)
{
    int32_t dAngle=0;
    if(motor->Angle-motor->LastAngle<-30000)
        dAngle=motor->Angle+(65535-motor->LastAngle);
    else if(motor->Angle-motor->LastAngle>30000)
        dAngle=-motor->LastAngle-(65535-motor->Angle);
    else
        dAngle=motor->Angle-motor->LastAngle;
    //将角度增量加入计数器
    motor->TotalAngle+=dAngle;
    //计算速度
    motor->Speed = (float)dAngle/(4*13*48)*50*60;//rpm   *50*60与定时器频率匹配
    //记录角度
    motor->LastAngle=motor->Angle;
}


/********************外部调用函数*****************/

//开始计算电机累计角度
void Motor_motorTt_StartCalcAngle(Motor_TT *motor)
{
    motor->TotalAngle=0;
    motor->LastAngle=motor->Angle;
    motor->TargetAngle=0;
}

//获取马达速度角度
void Motor_motorTt_GetSpeed(Motor_TT *motor)  //定时器调用
{

    motor[0].Angle=__HAL_TIM_GET_COUNTER(&htim1);
    motor[1].Angle=__HAL_TIM_GET_COUNTER(&htim8);
    Motor_motorTt_Cal(&motor[0]);
    Motor_motorTt_Cal(&motor[1]);

}

//设置马达pwm
void Motor_motorTt_SetPWM(float PWM1,float PWM2)
{
    if(PWM1>0)
    {
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, PWM1);
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, 0);
    }
    else
    {
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, 0);
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2,ABS(PWM1));
    }

    if(PWM2>0)
    {
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, PWM2);
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, 0);
    }
    else
    {
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, 0);
                __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, ABS(PWM2));
    }
}
//数字舵机设置角度
uint8_t Motor_digital_servo_setAngle(Servo *servo,uint16_t angle)
{
    servo->is_digital = 1;
    uint16_t width_pulse;
    if(angle > 180)
        angle = 180;
    width_pulse = 1000+(angle*1000)/180;
    if(servo->is_digital){
        __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,width_pulse);
        //HAL_Delay(500);
    }

    return true;

}
//总线电机角度发送
/**********************************************
	* @总线设备基本控制指令表：
	*	1.	#000PID!						//读取ID指令
	*	2.	#000PID001!						//设置ID指令
	*	3.	#000PVER!						//读取版本
	*	4.	#000PBD1!						//设置波特率 默认115200  1:9600 , 2:19200 , 3:38400 , 4:57600 , 5:115200 , 6:128000 7:256000  8:1000000
	*	5.	#000PCLE!						//恢复出厂设置包括ID
	* @总线舵机ZServo控制指令表：
	*	1.	#000P1500T1000!					//舵机角度控制
	*	2.	#000PDST!						//停止
	*	3.	#000PDPT!						//暂停
	*	4.	#000PDCT!						//继续
	*	5.	#000PCSD!						//设置当前值为开机值
	*	6.	#000PCSM!						//开机释放扭力
	*	7.	#000PRAD!						//读取角度
	*	8.	#000PULK!						//释力
	*	9.	#000PULR!						//恢复扭力
	*	10.	#000PSCK!						//设置当前值为1500的偏差值
	*	11.	#000PSCK+050!					//把1500+50作为1500的中间值
	*	12.	#000PSCK-050!					//把1500-50作为1500的中间值
	*	13.	#000PMOD!						//读取模式
	*	14.	#000PMOD1!						//设置模式 舵机模式：2 270 逆时针 1：270 顺 4 180 逆时针 3：180 顺    马达模式：6 360 逆时针 5：360 顺圈 8 360 逆时针 7：360 顺时
	*	15.	#000PRTV!						//读取电压和温度
	*	16.	#000PSTB!						//读取保护值
	*	17.	#000PSTB=60!					//设置保护值 默认60 范围25-80
	*	18.	#000PPAAAIBBB!					//设置KP = AAA, KI = BBB
	*	19.	#000PMIN!						//设置最小值
	*	20.	#000PMAX!						//设置最大值
	*	21.	#000PULM! 						//释力 不带阻力
	*	22.	#000PLN!						//RGB灯开启
	*	23.	#000PLF!						//RGB灯关闭
 **********************************************/
uint8_t Motor_bus_servo_setAngle(Servo *servo,uint8_t id,uint16_t angle)
{
    servo->is_digital = 0;//总线舵机
    uint16_t servo_buf[20]={0};

    //限幅
    if(angle < 500) angle = 500;
    else if(angle > 2500) angle = 2500;
    uint16_t pos = (angle*2000)/180 + 500;
    sprintf(servo_buf,"#%dP%dT100\r\n",id,pos);
    HAL_UART_Transmit_DMA(&huart1,servo_buf, strlen(servo_buf));
    return true;
}
//指定舵机ID
uint8_t Motor_bus_servo_setID(uint8_t id)
{
    uint16_t servo_rx_buf[20]={0},servo_tx_buf[20]={0};
    uint8_t status_rx = 0,status_tx = 0;
    sprintf(servo_tx_buf,"#1PID00%d",id);

    status_tx = HAL_UART_Transmit(&huart1, servo_tx_buf, strlen(servo_tx_buf), 0xffff);
    if (status_tx != HAL_OK) {
        return id;
    }
    status_rx = HAL_UART_Receive(&huart1, servo_rx_buf, strlen(servo_rx_buf), 0xffff);
    if (status_rx != HAL_OK) {
        return id;
    }
    if (*servo_rx_buf == id) {
        return id;
    } else {
        return HAL_ERROR;
    }

}


